#ifndef __UDSF_POINTRST_C_
#define __UDSF_POINTRST_C_

#include "udf_global.h"

/*
*   notice:
*   UDSF 同UDF编程：不可调用cstd-lib,只可调用"sdk_ifs_udk_cfg.h"、"sys_api.h"中列出的API函数。
*   UDSF文件内函数均需使用static修饰符，且UDF中以#include "UDSF.c"形式引用
*/

#include "udsf_cstdlike.c"

static void udsf_pointrst_user_process_demo(STRUCT_POINTRST *frameRst,STRUCT_POINTRST *frameRst_src0,STRUCT_POINTRST *frameRst_src1,int sn,PARAM_RSV* paramRSV) __attribute__((unused));


#define STRUCT_DEF \
struct{ \
uint32_t pow;\
int16_t  rIdx;\
int8_t   vIdx;\
int8_t   sinPhiIdx;\
uint32_t presence_flag;\
}

#define MAX_EPC_NUM       (32)
#define FILTER_NUM        (10)
#define FILTER_MEAN_NUM   (5)
#define HOLD_NUM          (10)

typedef STRUCT_DEF epc_unit_t;
struct epc
{
    int target_cnt;
    struct{
        STRUCT_DEF;
				uint16_t unprocess_flag;
    }target[MAX_EPC_NUM];
};

struct filter{
    uint8_t isempty[FILTER_NUM];
    struct {
				STRUCT_DEF;
				uint16_t valid;
    } target[FILTER_NUM][FILTER_MEAN_NUM+1];
};

struct hold{
    uint16_t cnt;
    struct {
        uint32_t tick_rcd;
        uint32_t presence_tick_rcd;
        uint32_t valid;
        epc_unit_t data;
    } target[HOLD_NUM][1];//+MAX_EPC_EXT_NUM];
};

static void udsf_epc_to_pcrst(struct epc *p_epc,STRUCT_POINTRST *frameRst,int vel_num){
	
	int i;
	
	for( i=0;i<p_epc->target_cnt;i++ ){
		
		if( i < FRAME_MAX_NUM ){
			frameRst->target[i].d1Idx = p_epc->target[i].rIdx;
			
			if( p_epc->target[i].vIdx < 0 ){
				frameRst->target[i].d2Idx = p_epc->target[i].vIdx + vel_num;
			}else{
				frameRst->target[i].d2Idx = p_epc->target[i].vIdx;
			}
			if( frameRst->target[i].d2Idx > (vel_num-1) ){
				frameRst->target[i].d2Idx = vel_num-1;
			}
			
			if( p_epc->target[i].sinPhiIdx < 0 ){
				frameRst->target[i].d3Idx = p_epc->target[i].sinPhiIdx + 64;
			}else{
				frameRst->target[i].d3Idx = p_epc->target[i].sinPhiIdx;
			}
			if( frameRst->target[i].d3Idx > 63 ){
				frameRst->target[i].d3Idx = 63;
			}
			
			frameRst->target[i].powABS = p_epc->target[i].pow;
			if( p_epc->target[i].presence_flag == 0 ){
				frameRst->target[i].powABS &= ~1;
			}else{
				frameRst->target[i].powABS |= 1;
			}
		}else{
			break;
		}
	}
	
	frameRst->targetNum = i;
}


static void udsf_pcrst_append_epc(STRUCT_POINTRST *frameRst,struct epc *p_epc,int vel_num,int presence_flag){
	
	for( int i=0;i<frameRst->targetNum;i++ ){
		
		if( p_epc->target_cnt < MAX_EPC_NUM ){
			
			p_epc->target[p_epc->target_cnt].rIdx = frameRst->target[i].d1Idx;
			
			if (frameRst->target[i].d2Idx >= vel_num/2){
				p_epc->target[p_epc->target_cnt].vIdx = frameRst->target[i].d2Idx - vel_num;
			}else{
				p_epc->target[p_epc->target_cnt].vIdx = frameRst->target[i].d2Idx;
			}
			
			if (frameRst->target[i].d3Idx >= NUM_ANGLE/2){
				p_epc->target[p_epc->target_cnt].vIdx = frameRst->target[i].d3Idx - NUM_ANGLE;
			}else{
				p_epc->target[p_epc->target_cnt].sinPhiIdx = frameRst->target[i].d3Idx;
			}

			p_epc->target[p_epc->target_cnt].pow = frameRst->target[i].powABS;

			p_epc->target[p_epc->target_cnt].presence_flag = presence_flag;

			p_epc->target_cnt++;
		}else{
			break;
		}
	}
}

static void udsf_cluster_append_epc(DBSCANOUT *DBscanClu ,struct epc *p_epc,int presence_flag){
	
	for( int i=0;i<DBscanClu->numCluster;i++ ){
		if( p_epc->target_cnt < MAX_EPC_NUM ){
			
        p_epc->target[p_epc->target_cnt].rIdx      = DBscanClu->cluster[i].rIdxMean>>12;
        p_epc->target[p_epc->target_cnt].vIdx      = DBscanClu->cluster[i].vIdxMean>>12;
        p_epc->target[p_epc->target_cnt].sinPhiIdx = DBscanClu->cluster[i].sinPhiIdxMean>>12;

        p_epc->target[p_epc->target_cnt].pow       = DBscanClu->cluster[i].abs;
        p_epc->target[p_epc->target_cnt].presence_flag = presence_flag;
			
				p_epc->target_cnt++;
    }else{
			break;
		}
	}
}

static int udsf_filter(struct filter *p_filter,struct epc *p_epc,uint32_t sn,PARAM_RSV *paramRSV)
{   
		uint8_t FILTER_RTH               = paramRSV->motion_filter_d1th;
		uint8_t FILTER_VTH               = paramRSV->motion_filter_d2th;
		uint8_t FILTER_ATH               = paramRSV->motion_filter_d3th;
		uint8_t FILTER_VALID_CNT         = paramRSV->motion_filter_valid_cnt;
	
    // clear all p_filter at current sn
    for( int i = 0;i< FILTER_NUM;i++ ){
        p_filter->target[i][sn%FILTER_MEAN_NUM].valid = 0;
    }

    for( int icluster=0;icluster<p_epc->target_cnt;icluster++ ){
        p_epc->target[icluster].unprocess_flag = 1;
			
        if( p_epc->target[icluster].presence_flag ){// filter不使用静态点
            p_epc->target[icluster].unprocess_flag = 0;
            continue;
        }
        
        for( int i = 0;i< FILTER_NUM;i++ ){
            int flag = 0;
            if( p_filter->isempty[i] ){
                continue;
            }
						int absVal1 = (p_epc->target[icluster].rIdx > p_filter->target[i][FILTER_MEAN_NUM].rIdx)?(p_epc->target[icluster].rIdx - p_filter->target[i][FILTER_MEAN_NUM].rIdx):(p_filter->target[i][FILTER_MEAN_NUM].rIdx -  p_epc->target[icluster].rIdx);
            int absVal2 = (p_epc->target[icluster].vIdx > p_filter->target[i][FILTER_MEAN_NUM].vIdx)?(p_epc->target[icluster].vIdx - p_filter->target[i][FILTER_MEAN_NUM].vIdx):(p_filter->target[i][FILTER_MEAN_NUM].vIdx -  p_epc->target[icluster].vIdx);
						int absVal3 = (p_epc->target[icluster].sinPhiIdx > p_filter->target[i][FILTER_MEAN_NUM].sinPhiIdx)?(p_epc->target[icluster].sinPhiIdx - p_filter->target[i][FILTER_MEAN_NUM].sinPhiIdx):(p_filter->target[i][FILTER_MEAN_NUM].sinPhiIdx -  p_epc->target[icluster].sinPhiIdx);
						
						if( p_filter->target[i][FILTER_MEAN_NUM].valid ){ // cover均值
							if( (absVal1 < FILTER_RTH) && (absVal2 < FILTER_VTH) && (absVal3 < FILTER_ATH) ){
								p_epc->target[icluster].unprocess_flag = 0;
								flag = 1;
							}
						}else{
							for( int j=0;j<FILTER_MEAN_NUM;j++ ){
								int index = (sn+FILTER_MEAN_NUM-j)%FILTER_MEAN_NUM;
								absVal1 = (p_epc->target[icluster].rIdx > p_filter->target[i][index].rIdx)?(p_epc->target[icluster].rIdx - p_filter->target[i][index].rIdx):(p_filter->target[i][index].rIdx - p_epc->target[icluster].rIdx);
								absVal2 = (p_epc->target[icluster].vIdx > p_filter->target[i][index].vIdx)?(p_epc->target[icluster].vIdx - p_filter->target[i][index].vIdx):(p_filter->target[i][index].vIdx - p_epc->target[icluster].vIdx);
								absVal3 = (p_epc->target[icluster].sinPhiIdx > p_filter->target[i][index].sinPhiIdx)?(p_epc->target[icluster].sinPhiIdx - p_filter->target[i][index].sinPhiIdx):(p_filter->target[i][index].sinPhiIdx - p_epc->target[icluster].sinPhiIdx);

								if( p_filter->target[i][index].valid &&  // 无有效均值,cover上个有效值
								 (absVal1 < FILTER_RTH) && (absVal2 < FILTER_VTH) && (absVal3 < FILTER_ATH) ){
										p_epc->target[icluster].unprocess_flag = 0;
										flag = 1;
										break;
								}
							}
            }

            if( flag ){  
                p_filter->isempty[i] = 0;
								*((epc_unit_t*)&p_filter->target[i][sn%FILTER_MEAN_NUM]) = *((epc_unit_t*)&p_epc->target[icluster]);
                p_filter->target[i][sn%FILTER_MEAN_NUM].valid            = 1;
                //break;
            }
        }
    }

    for( int i=0;i<FILTER_NUM;i++ ){ // 均值滤波

			  int d1_mean = 0;
        int d2_mean = 0;
        int d3_mean = 0;
        int pow     = 0;
			
        if( p_filter->isempty[i] ){
            continue;
        }

				int valid_cnt = 0;
				
        for( int j=0;j<FILTER_MEAN_NUM;j++ ){
					if( p_filter->target[i][j].valid ){

						valid_cnt ++;
							
						d1_mean += p_filter->target[i][j].rIdx;
						d2_mean += p_filter->target[i][j].vIdx;
						d3_mean += p_filter->target[i][j].sinPhiIdx;
						if( (uint32_t)(pow + p_filter->target[i][j].pow) > pow ){
							pow += p_filter->target[i][j].pow;
						}
					}
        }
				
				if( valid_cnt == 0 ){
            p_filter->isempty[i] = 1;
            continue;
        }

        if( valid_cnt >= FILTER_VALID_CNT ){
						
            d1_mean = d1_mean*paramRSV->divFac.value[valid_cnt-1];
            d2_mean = d2_mean*paramRSV->divFac.value[valid_cnt-1];
            d3_mean = d3_mean*paramRSV->divFac.value[valid_cnt-1];
            pow     = pow*paramRSV->divFac.value[valid_cnt-1];

						p_filter->target[i][FILTER_MEAN_NUM].rIdx          = d1_mean>>12;
						p_filter->target[i][FILTER_MEAN_NUM].vIdx          = d2_mean>>12;
						p_filter->target[i][FILTER_MEAN_NUM].sinPhiIdx     = d3_mean>>12;
						p_filter->target[i][FILTER_MEAN_NUM].pow           = pow>>12;
						p_filter->target[i][FILTER_MEAN_NUM].presence_flag = 0;
            p_filter->target[i][FILTER_MEAN_NUM].valid         = 1;
        }else{
            p_filter->target[i][FILTER_MEAN_NUM].valid         = 0;
        }
    }

    // 添加新点
    for( int icluster=0;icluster<p_epc->target_cnt;icluster++ ){
			if( p_epc->target[icluster].unprocess_flag ){
				for( int i=0;i<FILTER_NUM;i++ ){
					if( p_filter->isempty[i] ){						
						*((epc_unit_t*)&p_filter->target[i][sn%FILTER_MEAN_NUM]) = *((epc_unit_t*)&p_epc->target[icluster]);
						
						p_filter->target[i][sn%FILTER_MEAN_NUM].valid         = 1;
						p_filter->isempty[i] = 0;
						break;
					}
				}
			}
    }

    {
        int new_cnt = 0;
        for( int i=0;i<p_epc->target_cnt;i++ ){
            if( p_epc->target[i].presence_flag ){
                if( new_cnt != i ){
                    p_epc->target[new_cnt] = p_epc->target[i];
                }
                new_cnt ++;
            }
        }
        p_epc->target_cnt = new_cnt;
    }

    for( int i=0;i<FILTER_NUM;i++ ){
			if( !p_filter->isempty[i] && p_filter->target[i][FILTER_MEAN_NUM].valid ){
				if( p_epc->target_cnt < MAX_EPC_NUM  ){
						*((epc_unit_t*)&p_epc->target[p_epc->target_cnt]) = *((epc_unit_t*)&p_filter->target[i][FILTER_MEAN_NUM]);
						p_epc->target_cnt++;
				}else{
						break;
				}
			}
    }

    return 0;
}

static void udsf_hold(struct hold *p_hold,struct epc *p_epc,uint32_t tick,PARAM_RSV* paramRSV)
{
    int32_t d1th,d2th,d3th,tick_hold;

		uint32_t presence_ignore = (paramRSV->motion_hold_sec*((FRAME_PER_SEC/POINTRST_USER_PROCESS_PER_N_FRAME)/2));
	
    // 刷旧点
    for( int i = 0;i<p_epc->target_cnt;i++ ){
        if( p_epc->target[i].presence_flag ){
            d1th      = paramRSV->presence_hold_d1th;
            d2th      = paramRSV->presence_hold_d2th;
            d3th      = paramRSV->presence_hold_d3th;
            tick_hold = paramRSV->presence_hold_sec;
        }else{
            d1th      = paramRSV->motion_hold_d1th;
            d2th      = paramRSV->motion_hold_d2th;
            d3th      = paramRSV->motion_hold_d3th;
            tick_hold = paramRSV->motion_hold_sec;
        }

        int flag = 0;
        for( int j=0;j<p_hold->cnt;j++ ){
					int absVal1 = (p_epc->target[i].rIdx > p_hold->target[j][0].data.rIdx )?(p_epc->target[i].rIdx - p_hold->target[j][0].data.rIdx):( p_hold->target[j][0].data.rIdx - p_epc->target[i].rIdx);
					int absVal2 = (p_epc->target[i].rIdx > p_hold->target[j][0].data.rIdx )?(p_epc->target[i].rIdx - p_hold->target[j][0].data.rIdx):( p_hold->target[j][0].data.rIdx - p_epc->target[i].rIdx);
					int absVal3 = (p_epc->target[i].rIdx > p_hold->target[j][0].data.rIdx )?(p_epc->target[i].rIdx - p_hold->target[j][0].data.rIdx):( p_hold->target[j][0].data.rIdx - p_epc->target[i].rIdx);
            if( (absVal1<d1th) && (absVal2<d2th) && (absVal3<d3th) ){                
                if( !p_epc->target[i].presence_flag ) // 动点无条件更新
                {
                    p_hold->target[j][0].data = *((epc_unit_t*)&p_epc->target[i]);
                    p_hold->target[j][0].tick_rcd = tick + tick_hold*(FRAME_PER_SEC/POINTRST_USER_PROCESS_PER_N_FRAME);
                }

                // 距离动态点时间太近的静态点不使用
                if( (p_epc->target[i].presence_flag) && (p_hold->target[j][0].data.presence_flag==0) &&
                    (p_hold->target[j][0].tick_rcd > tick) && (p_hold->target[j][0].tick_rcd - tick) > presence_ignore ){
                    continue;
                }

                flag = 1;

                // 静cover静 仍然标记为静点
                if( p_epc->target[i].presence_flag && p_hold->target[j][0].data.presence_flag ){
                    p_hold->target[j][0].tick_rcd = tick + tick_hold*(FRAME_PER_SEC/POINTRST_USER_PROCESS_PER_N_FRAME);
										//LOG_SEND_STRING(LOG_LEVEL_DEBUG,"presence hold\n");
                }else{
                    if( !p_epc->target[i].presence_flag ){ // 动cover动/静
                        p_hold->target[j][0].data.presence_flag = 0; // 动点
                        p_hold->target[i][0].presence_tick_rcd = tick;
                    }else{ // 静cover动
                        p_hold->target[j][0].data.presence_flag = 1; // 静cover动
                    }
										//LOG_SEND_STRING(LOG_LEVEL_DEBUG,"xxx hold\n");
                    p_hold->target[j][0].tick_rcd = tick + tick_hold*(FRAME_PER_SEC/POINTRST_USER_PROCESS_PER_N_FRAME);
                }
                //break;
            }
        }
        if( flag ){
            p_epc->target[i].unprocess_flag = 0;
        }else{
            p_epc->target[i].unprocess_flag = 1;
        }
    }
    // 去过期点
    int new_cnt = 0;
    for( int i=0;i<p_hold->cnt;i++ ){
			_Bool xx = (p_hold->target[i][0].data.presence_flag) && 
			(p_hold->target[i][0].presence_tick_rcd + paramRSV->presence_timeout_sec*(FRAME_PER_SEC/POINTRST_USER_PROCESS_PER_N_FRAME) < tick);

			if( (p_hold->target[i][0].tick_rcd>tick) &&
					(!xx) ){
					if( new_cnt != i ){
						p_hold->target[new_cnt][0] = p_hold->target[i][0];
					}
					new_cnt++;
			}
    }
    p_hold->cnt = new_cnt;

    // 增新动点
    for( int i = 0;i<p_epc->target_cnt;i++ ){
        if( (p_epc->target[i].unprocess_flag == 1) && (!p_epc->target[i].presence_flag) ){

            if( p_hold->cnt < HOLD_NUM ){

                p_hold->target[p_hold->cnt][0].data = *((epc_unit_t*)&p_epc->target[i]);
                p_hold->target[p_hold->cnt][0].tick_rcd = tick + tick_hold*(FRAME_PER_SEC/POINTRST_USER_PROCESS_PER_N_FRAME);
                p_hold->target[p_hold->cnt][0].presence_tick_rcd = tick;

                p_hold->cnt++;
            }
        }
    }

    // hold to epc
    for( int i = 0;i<p_hold->cnt;i++ ){
        *((epc_unit_t*)&p_epc->target[i]) = p_hold->target[i][0].data;
    }
    p_epc->target_cnt = p_hold->cnt;
}


static void udsf_pointrst_user_process_demo(STRUCT_POINTRST *frameRst,STRUCT_POINTRST *frameRst_src0,STRUCT_POINTRST *frameRst_src1,int sn,PARAM_RSV* paramRSV)
{
	struct filter *p_filter = (struct filter *) UDFDEF_ADDR_FILTER_STRUCT;
	struct hold   *p_hold   = (struct hold *)   UDFDEF_ADDR_HOLD_STRUCT;
	struct dbscan *pDBSCAN  = (struct dbscan *) UDFDEF_ADDR_DBSCAN_BASE;
	struct epc    *p_epc    = (struct epc *)    UDFDEF_ADDR_EPC_STRUCT;
	

	if( (sizeof( struct dbscan) > UDFDEF_SIZE_DBSCAN_STRUCT) ||  
		  (sizeof( struct filter) > UDFDEF_SIZE_FILTER_STRUCT) || 
			(sizeof( struct hold) > UDFDEF_SIZE_HOLD_STRUCT) || 
			(sizeof( struct epc) > UDFDEF_SIZE_EPC_STRUCT	)){
		while(1){
			udsf_delay_ms(500);
			LOG_SEND_STRING(LOG_LEVEL_EMERG,"struct dbscan too large\n");
			uint32_t size = sizeof( struct dbscan);
			LOG_SEND_U32_BASE16(LOG_LEVEL_EMERG,1,&size,1,"dbscan size: ","\n");
			
			size = sizeof(struct filter);
			LOG_SEND_U32_BASE16(LOG_LEVEL_EMERG,1,&size,1,"filter size: ","\n");
			
			size = sizeof(struct hold);
			LOG_SEND_U32_BASE16(LOG_LEVEL_EMERG,1,&size,1,"hold size: ","\n");
			
			size = sizeof(struct epc);
			LOG_SEND_U32_BASE16(LOG_LEVEL_EMERG,1,&size,1,"epc size: ","\n");
		}
	}
	
	frameRst->targetNum = 0;
	
	p_epc->target_cnt = 0;
	
	//paramRSV->dbscan_mean_buffer_num = 4; // todo
	
	#if 1
	// 动点聚类	
	if( frameRst_src0->targetNum ){
		DBSCANCFG xx;
		xx.minPts        = paramRSV->motion_cluster_minpts;
		xx.epsRIdx       = paramRSV->motion_cluster_d1th;
		xx.epsVIdx       = paramRSV->motion_cluster_d2th;
		xx.epsSinPhiIdx  = paramRSV->motion_cluster_d3th;
		udsf_dbscan(pDBSCAN,paramRSV->divFac.value,&xx,frameRst_src0,UDFDEF_CHIRP_NUM);
		udsf_cluster_append_epc(&pDBSCAN->dbscanClu,p_epc,0);
	}
	
	// 静点聚类
	if( frameRst_src1->targetNum ){
		DBSCANCFG xx;
		xx.minPts        = paramRSV->presence_cluster_minpts;
		xx.epsRIdx       = paramRSV->presence_cluster_d1th;
		xx.epsVIdx       = paramRSV->presence_cluster_d2th;
		xx.epsSinPhiIdx  = paramRSV->presence_cluster_d3th;
		udsf_dbscan(pDBSCAN,paramRSV->divFac.value,&xx,frameRst_src1,UDFDEF_CHIRP_NUM);
		udsf_cluster_append_epc(&pDBSCAN->dbscanClu,p_epc,1);
	}
	#else // just for test
	udsf_pcrst_append_epc(frameRst_src0,p_epc,UDFDEF_CHIRP_NUM,0);
	udsf_pcrst_append_epc(frameRst_src1,p_epc,UDFDEF_CHIRP_NUM,1);
	#endif
	
	if( sn == 0 ){ // 初始化
		for( int i=0;i<FILTER_NUM;i++ ){
			for( int j=0;j<FILTER_MEAN_NUM+1;j++ ){
					p_filter->target[i][j].valid = 0;
			}
			p_filter->isempty[i] = 1;
		}
		p_hold->cnt = 0;
	}
	
	//LOG_SEND_U32_BASE16(LOG_LEVEL_DEBUG,1,(uint32_t*)&sn,1,"sn:","\n");

	udsf_filter(p_filter,p_epc,sn,paramRSV);
	udsf_hold(p_hold,p_epc,sn,paramRSV);
	udsf_epc_to_pcrst(p_epc,frameRst,UDFDEF_CHIRP_NUM);
	
	#if (FFT2D_MINUS_MEAN_MODE == 1) && defined(MINUS_MODE_SWITCH)
	if( p_epc->target_cnt == 0 ){
		if( ++sUDF_global.no_target_cnt > paramRSV->no_target_timeout_sec*(FRAME_PER_SEC/POINTRST_USER_PROCESS_PER_N_FRAME) ){  // N s后切为减本帧
			sUDF_global.minus_old_frame_enable = 0;
			#if defined(LED_ENABLE)
			GPIO0_4 = 1;
			#endif
		}
	}else{
		sUDF_global.no_target_cnt = 0;
		sUDF_global.minus_old_frame_enable = 1;
		#if defined(LED_ENABLE)
		GPIO0_4 = 0;
		#endif
	}
	#endif
}

#endif
